#include "HardwareSerial.h"
#include "pinmap.h"
#include "PeripheralPins.h"

#ifdef HAVE_HWSERIAL1
HardwareSerial Serial1(USART1);
void serialEvent1() __attribute__((weak));
#endif

#ifdef HAVE_HWSERIAL2
HardwareSerial Serial2(USART2);
void serialEvent2() __attribute__((weak));
#endif

#ifdef HAVE_HWSERIAL3
HardwareSerial Serial3(USART3);
void serialEvent3() __attribute__((weak));
#endif

HardwareSerial::HardwareSerial(USART_TypeDef *peripheral)
{
    _usert_mode = 0;

    PinName tx_ = pinmap_pin(peripheral, PinMap_UART_TX);
    PinName rx_ = pinmap_pin(peripheral, PinMap_UART_RX);

    initUart(peripheral, rx_, tx_);
}

HardwareSerial::HardwareSerial(uint32_t _rx, uint32_t _tx)
{
    init(digitalPinToPinName(_rx), digitalPinToPinName(_tx));
}

HardwareSerial::HardwareSerial(PinName _rx, PinName _tx)
{
    init(_rx, _tx);
}

HardwareSerial::HardwareSerial(uint32_t _rxtx)
{
    init(NC, digitalPinToPinName(_rxtx));
}

HardwareSerial::HardwareSerial(PinName _rxtx)
{
    init(NC, _rxtx);
}

void HardwareSerial::begin(unsigned long baud, uint8_t config)
{
    uint32_t databits = 0;
    uint32_t stopbits = 0;
    uint32_t parity = 0;

    _baud = baud;
    _config = config;

    switch (config & 0x07)
    {
    case 0x02:
        databits = 6;
        break;
    case 0x04:
        databits = 7;
        break;
    case 0x06:
        databits = 8;
        break;
    default:
        databits = 0;
        break;
    }

    if ((config & 0x30) == 0x30)
    {
        parity = USART_Parity_Odd;
        databits++;
    }
    else if ((config & 0x20) == 0x20)
    {
        parity = USART_Parity_Even;
        databits++;
    }
    else
    {
        parity = USART_Parity_No;
    }

    if ((config & 0x08) == 0x08)
    {
        stopbits = USART_StopBits_2;
    }
    else
    {
        stopbits = USART_StopBits_1;
    }

    switch (databits)
    {
#ifdef UART_WORDLENGTH_7B
    case 7:
        databits = USART_WordLength_7b;
        break;
#endif
    case 8:
        databits = USART_WordLength_8b;
        break;
    case 9:
        databits = USART_WordLength_9b;
        break;
    default:
    case 0:
        Error_Handler();
        break;
    }

    USART_InitTypeDef USART_InitStructure;
    NVIC_InitTypeDef NVIC_InitStructure;

    USART_InitStructure.USART_BaudRate = baud;
    USART_InitStructure.USART_WordLength = databits;
    USART_InitStructure.USART_StopBits = stopbits;
    USART_InitStructure.USART_Parity = parity;
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
    USART_InitStructure.USART_Mode = _usert_mode;

    USART_Init(_usert_x, &USART_InitStructure);

    USART_ITConfig(_usert_x, USART_IT_RXNE, ENABLE);

    if (_usert_x == USART1)
    {
        NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
        NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
        NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    }
    else if (_usert_x == USART2)
    {
        NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
        NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
        NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    }
    else if (_usert_x == USART3)
    {
        NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
        NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
        NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    }
    NVIC_Init(&NVIC_InitStructure);

    USART_Cmd(_usert_x, ENABLE);
}

void HardwareSerial::end()
{
    if (_usert_x != NULL)
    {
        USART_Cmd(_usert_x, DISABLE);
        USART_DeInit(_usert_x);
    }
}

int HardwareSerial::available(void)
{
    return _rxBuffer.available();
}

int HardwareSerial::peek(void)
{
    return _rxBuffer.peek();
}

int HardwareSerial::read(void)
{
    return _rxBuffer.read_char();
}

// int HardwareSerial::availableForWrite(void)
// {
//     while (USART_GetFlagStatus(_usert_x, USART_FLAG_TC) == RESET)
//         ;
// }

void HardwareSerial::flush(void)
{
}

size_t HardwareSerial::write(uint8_t c)
{

    USART_SendData(_usert_x, c);
    while (USART_GetFlagStatus(_usert_x, USART_FLAG_TXE) == RESET)
        ;
    return 1;
}

void HardwareSerial::IrqHandler()
{
    _rxBuffer.store_char(USART_ReceiveData(_usert_x));
}

void HardwareSerial::init(PinName _rx, PinName _tx)
{
    PinName tx_, rx_;
    if (_rx == _tx)
        rx_ = NC;
    else
        rx_ = _rx;

    tx_ = _tx;

    USART_TypeDef *uart_tx = (USART_TypeDef *)pinmap_peripheral(tx_, PinMap_UART_TX);
    USART_TypeDef *uart_rx = (USART_TypeDef *)pinmap_peripheral(rx_, PinMap_UART_RX);

    if (uart_tx == NP)
        return;

    if (rx_ != NC && uart_rx == NP)
        return;

    initUart(uart_tx, rx_, tx_);
}

void HardwareSerial::initUart(USART_TypeDef *peripheral, uint32_t rx_, uint32_t tx_)
{
    if (peripheral == USART1)
    {
        RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1 | RCC_APB2Periph_GPIOA, ENABLE);
    }
    else if (peripheral == USART2)
    {
        RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
    }
    else if (peripheral == USART3)
    {
        RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
    }
    else
    {
        Error_Handler();
    }

    GPIO_InitTypeDef GPIO_InitStructure;

    if (tx_ != NC)
    {
        GPIO_InitStructure.GPIO_Pin = REF_GPIO_PIN(tx_);
        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
        GPIO_Init(REF_GPIO_PORT(tx_), &GPIO_InitStructure);
    }
    if (rx_ != NC)
    {
        GPIO_InitStructure.GPIO_Pin = REF_GPIO_PIN(rx_);
        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
        GPIO_Init(REF_GPIO_PORT(rx_), &GPIO_InitStructure);
    }
    else
    {
        USART_HalfDuplexCmd(peripheral, ENABLE);
    }
    _usert_mode = USART_Mode_Tx | USART_Mode_Rx;
    _usert_x = peripheral;
}

WEAK void serialEventRun(void)
{
#ifdef HAVE_HWSERIAL1
    if (serialEvent1 && Serial1.available())
        serialEvent1();
#endif
#ifdef HAVE_HWSERIAL2
    if (serialEvent2 && Serial2.available())
        serialEvent2();
#endif
#ifdef HAVE_HWSERIAL3
    if (serialEvent3 && Serial3.available())
        serialEvent3();
#endif
#ifdef HAVE_HWSERIAL4
    if (serialEvent4 && Serial4.available())
        serialEvent4();
#endif
#ifdef HAVE_HWSERIAL5
    if (serialEvent5 && Serial5.available())
        serialEvent5();
#endif
}

#ifdef __cplusplus
extern "C"
{
#endif
#ifdef HAVE_HWSERIAL1
    void __func_interrupt USART1_IRQHandler(void)
    {
        if (USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
        {
            Serial1.IrqHandler();
        }
    }
#endif

#ifdef HAVE_HWSERIAL2
    void __func_interrupt USART2_IRQHandler(void)
    {
        if (USART_GetITStatus(USART2, USART_IT_RXNE) != RESET)
        {
            Serial2.IrqHandler();
        }
    }
#endif

#ifdef HAVE_HWSERIAL3
    void __func_interrupt USART3_IRQHandler(void)
    {
        if (USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)
        {
            Serial3.IrqHandler();
        }
    }
#endif

#ifdef __cplusplus
}
#endif